# -*- coding: utf-8 -*-

import casadi

from elp import *
from short import *
import const

RMAX = 0.35
RMIN = 0.15

split_d = 0.05

obstacle_points = np.zeros((0, 2))

for edge in obstacle_edge:
    p1 = points[edge[0]]
    p2 = points[edge[1]]
    d = np.linalg.norm(p1 - p2)
    n = int(d/split_d) + 1
    ps = np.zeros((n, 2))
    for i in range(2):
        ps[:, i] = np.linspace(p1[i], p2[i], n)
    obstacle_points = np.vstack((obstacle_points, ps))

obstacle_points = KDTree(obstacle_points)

def GetTargetSubjectPoint(here, there, subject_point=None, rmax=RMAX):
    poslst = GetPath(here, there)
    target = poslst.shape[0]-1
    for i in range(poslst.shape[0]-2, -1, -1):
        if np.linalg.norm(poslst[i] - here) <= rmax * 2:
            target = i + 1
            break
    target = poslst[target]
    o_pos = obstacle_points.query_ball_point(here, rmax*2+0.1)
    o_pos = obstacle_points.data[o_pos]
    if subject_point is not None:
        subject_point = np.vstack((subject_point, o_pos))
    else:
        subject_point = o_pos
    return target, subject_point

def GetTarget(here, there, subject_point=None, rmax=RMAX, rmin=RMIN, in_points=None):
    target, subject_point = GetTargetSubjectPoint(here, there, subject_point, rmax)
    opt = casadi.Opti()
    elp = Ellipse(opt)
    opt.minimize((elp.x0 - target[0])**2 + (elp.y0 - target[1])**2)
    opt.subject_to(elp.a <= rmax)
    opt.subject_to(elp.b >= rmin)
    opt.subject_to(elp(here) <= 0.0)
    if in_points is None:
        in_points = np.zeros((0, 2))
    for p in in_points:
        opt.subject_to(elp(p) <= 0.0)
    for p in subject_point:
        opt.subject_to(elp(p) > 0.0)
    elp.BeforeSolve()
    opt.set_initial(elp.x0, here[0])
    opt.set_initial(elp.y0, here[1])
    opt.solver('ipopt', const.OPTION)
    try:
        sol = opt.solve()
    except BaseException as e:
        print(e)
        return None, None
    elp.AfterSolve(sol)
    return np.array([elp.x0, elp.y0]), elp.Sample(np.linspace(0, np.pi*2, 20))


def GetTarget2(here, there, subject_point=None, rmax=RMAX, rmin=RMIN, in_points=None):
    rmax *= 2.0
    poslst = GetPath(here, there)
    target = poslst[1]
    for i in range(1, poslst.shape[0]):
        if np.linalg.norm(poslst[i] - here) > rmax:
            target = poslst[i]
            break
    d = np.linalg.norm(here - target)
    target = here + (target - here) * (rmax / d)
    elp = Ellipse(None)
    elp.x0 = target[0]
    elp.y0 = target[1]
    elp.yaw = 0.0
    elp.a = 1.0
    elp.b = 1.0
    return np.array([elp.x0, elp.y0]), elp.Sample(np.linspace(0, np.pi*2, 20))
